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1.  INTRODUCTION 

In  designing  a  suboptimal  Kalman  filter,  the  designer  must 
decide  how  to  simplify  the  system  error  model  without 
causing  the  filter  estimation  errors  to  increase  to 
unacceptable  levels.  Deletion  of  certain  error  states  and 
decoupling  of  error  state  dynamics  are  the  two  principal 
model  simplifications  that  are  commonly  used  in  suboptimal 
filter  design.  For  the  most  part,  the  decisions  as  to  which 
error  states  can  be  deleted  or  decoupled  are  based  on  the 
designer's  understanding  of  the  physics  of  the  particular 
system.  Consequently,  the  details  of  a  suboptimal  design 
are  usually  unique  to  the  specific  application. 

In  this  paper,  the  process  of  designing  a  suboptimal  Kalman 
filter  is  illustrated  for  the  case  of  an  airborne  tamsfer-af- 
alignment  (TO A)  system  used  for  synthetic  aperture  radar 
(SAR)  motion  compensation.  In  this  application,  the  filter 
must  continuously  transfer  the  alignment  of  an  onboard 
Doppler-damped  master  inertial  navigation  system  (INS)  to  a 
strapdown  navigator  that  processes  information  from  a  less 
accurate  inertial  measurement  nnit  (IMU)  mounted  on  the 
radar  antenna.  The  IMU  is  used  to  measure  spurious 
antenna  motion  during  the  SAR  imaging  interval,  so  that 
compensating  phase  corrections  can  be  computed  and 
applied  to  the  radar  returns,  thereby  preventing  image 
degradation  that  would  otherwise  result  from  such  motions. 
The  principles  of  SAR  are  described  in  many  references,  for 
instance  [1],  [2]. 

The  primary  function  of  the  TO  A  Kalman  filter  in  a  SAR 
motion  compensation  system  is  to  control  strapdown 
navigator  attitude  errors,  and  to  a  lesser  degree,  velocity 
and  heading  errors.  Unlike  a  classical  navigation 
application,  absolute  positional  accuracy  is  not  important. 
The  motion  compensation  requirements  for  SAR  imaging 
are  discussed  in  some  detail  in  [3].  This  TO  A  application  is 
particularly  appropriate  as  a  vehicle  for  discussing 
suboptimal  filter  design,  because  the  system  contains 
features  that  can  be  exploited  to  allow  both  deletion  and 
decoupling  of  error  states. 

In  Section  2,  a  high-level  background  description  of  a  SAR 
motion  compensation  system  that  incorporates  a  TOA 
Kalman  filter  is  given.  The  optimal  TOA  filter  design  is 
presented  in  Section  3  with  some  simulation  results  to 
indicate  potential  filter  performance.  In  Section  4,  the 
suboptimal  Kalman  filter  configuration  is  derived. 
Simulation  results  are  also  shown  in  this  section  to  allow 
comparison  between  suboptimal  and  optimal  filter 
performances.  Conclusions  are  contained  in  Section  5. 

Throughout  this  paper,  it  is  assumed  that  the  reader  is 
familiar  with  basic  Kalman  filter  theory.  If  not,  a  treatment 
of  this  subject  can  be  found  in  [4],  [5]  or  [6]. 


2.  SAR  MOTION  COMPENSATION  SYSTEM 
DESCRIPTION 

2.1  Sensors 

A  functional  block  diagram  of  a  SAR  motion  compensation 
system  (SARMCS)  that  utilizes  a  TOA  Kalman  filter  is 
shown  in  Figure  1 . 


Figure  1.  Functional  Block  Diagram  of  a  SARMCS 


The  principal  motion  compensation  sensor  is  a  small  /nertial 
measurement  wnit  (IMU)  mounted  directly  on  the  antenna 
structure.  It  provides  raw  measurements  of  the  angular 
rates  and  linear  accelerations  of  the  antenna  in  the  standard 
form  of  angular  and  velocity  increments.  The  antenna 
structure  is  assumed  to  be  physically  stabilized  in  roll  and 
pitch.  Consequently,  the  strapdown  IMU  is  exposed  to  only 
residual  angular  motion  about  the  aircraft’s  roll  and  pitch 
axes  resulting  from  imperfect  antenna  stabilization,  while  it 
is  fully  exposed  to  angular  motion  caused  by  aircraft 
heading  changes.  It  is  assumed  that  the  IMU  contains  two 
dry  tuned-rotor  gyroscopes  and  three  accelerometers  with 
the  specifications  shown  in  Table  1 . 

The  onboard  master  mertial  navigation  system  (INS)  is 
remotely  located  from  the  IMU.  It  is  assumed  to  be  a 
medium-accuracy  system  (1  nm/hr,  lex)  with  a  baroaltitude- 
damped  vertical  channel.  It  employs  three  ring  laser 
gyroscopes  and  three  accelerometers,  which  have  the 
characteristics  indicated  in  Table  2. 

The  Doppler  velocity  sensor  (DVS)  employs  a  standard 
strapdown  lambda  3 -beam  configuration  [7]  to  measure 
aircraft  velocity  in  aircraft  body  coordinates  relative  to  the 
terrain  surface.  The  primary  time-correlated  error  in  the 
measurement  of  the  forward  velocity  component  is  a  scale 
factor  type  error  in  the  order  of  1-2%  (lex),  depending  upon 
the  terrain.  The  error  in  measuring  the  lateral  velocity 
component  principally  results  from  an  antenna  boresight 
azimuth  misalignment  which  is  typically  less  than  1°  (lcr). 
In  addition  to  these  time-correlated  errors,  there  is  also  a 
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significant  noise  component  to  the  DVS  measurements  of 
velocity;  the  level  of  noise  is  typically  1  m/s  (la)  for  the 
forward  velocity  measurement,  and  1.5  m/s  (la)  for  the 
lateral  velocity  measurement.  It  is  assumed  that  the  DVS  is 
only  operated  over  land,  so  that  additional  errors  introduced 
by  the  effects  of  sea  currents  and  waves  can  be  ignored. 

The  air  data  sensors  (ADS)  include  a  pressure  transducer  to 
measure  static  air  pressure,  and  an  air  temperature  probe  to 
measure  outside  air  temperature.  This  information  is  used 
to  compute  baroaltitude. 


IMU  Gyro  Performance  Characteristics 

Characteristic 

Performance 

Scale  factor  repeatability 

150  ppm  (la) 

Bias  repeatability 

0.2  °/hr  (Icy) 

G-sensitive  drift  repeatability 

0.2  °/hr/g  (la) 

|  IMU  Accelerometer  Performance  Characteristics 

Characteristic 

Performance 

Scale  factor  repeatability 

200  ppm  (la) 

Bias  repeatability 

100  )ig  (Icy) 

Table  1.  IMU  Gyro  and  Accelerometer  Performance 
Characteristics. 


INS  Gyro  Performance  Characteristics 

Characteristic 

Performance 

Scale  factor  repeatability 

Bias  repeatability 

Random  drift 

5  ppm  (la) 

0.01  °/hr  (la) 

0.003 °/V  hr  (la) 

INS  Accelerometer  Performance  Characteristics 

Characteristic 

Performance 

Scale  factor  repeatability 

Bias  repeatability 

50  ppm  (lcr) 

50  pg  (la) 

Table  2.  INS  Gyro  and  Accelerometer  Performance 
Characteristics. 

2.2  Processing  Overview 

Velocity  and  angular  increments  from  the  IMU  are 
processed  in  a  strapdown  navigator  algorithm  which 
mechanizes  the  navigation  equations  in  a  wander  azimuth 
frame.  Since  the  IMU  is  mounted  directly  on  the  antenna, 
the  strapdown  algorithm  fundamentally  computes  antenna 
attitude  and  heading,  and  velocity  and  position  of  a 
particular  point  at  the  IMU  location.  A  lever  arm  correction 
is  applied  within  the  navigator  to  obtain  estimates  of  the 
velocity  and  position  of  the  antenna  phase  centre  which  is 


the  point  of  interest  for  SAR  motion  compensation.  The 
computed  baroaltitude  is  used  in  a  classical  third-order 
damping  loop  to  stabilize  the  strapdown  vertical  channel. 

The  strapdown  navigator  estimates  of  antenna  phase  centre 
motion  are  provided  to  a  targetting  algorithm  during 
intervals  of  SAR  imaging.  The  targetting  algorithm  initially 
computes  a  line-of-sight  vector  from  the  antenna  phase 
centre  to  the  target,  using  target  information  supplied  from 
other  parts  of  the  SAR  system.  This  line-of-sight  vector  is 
then  updated  throughout  the  imaging  interval.  At  each  point 
in  time,  the  targetting  algorithm  calculates  the  component  of 
the  antenna  phase  centre  velocity  lying  along  the  line-of- 
sight  vector,  and  integrates  this  velocity  component  in  time 
to  obtain  phase  centre  displacement  along  the  line-of-sight. 
From  this  displacement,  an  appropriate  value  of  phase 
correction  is  computed  for  each  point  in  time,  and  applied  to 
the  radar  signal  returns  to  compensate  for  the  spurious 
antenna  motion. 

The  outputs  of  the  strapdown  navigator,  along  with 
information  from  the  master  INS  and  DVS,  are  supplied  to  a 
Kalman  filter  which  estimates  various  system  and  sensor 
errors  associated  with  these  devices.  The  filter-estimated 
strapdown  navigation  errors  and  IMU  instrument  errors  are 
fed  back  to  the  strapdown  navigator  algorithm  and  used 
there  to  correct  the  relevant  quantities  in  a  closed  loop 
fashion.  The  net  result  of  this  Kalman  filter  implementation 
is  that  alignment  of  a  Doppler-damped  INS  is  continually 
transferred  to  the  strapdown  navigator. 

3.  OPTIMAL  TOA  KALMAN  FILTER  DESIGN 
3.1  State  Vector 

The  state  vector  in  the  optimal  TOA  Kalman  filter  design 
contains  26  states,  representing  all  significant  time- 
correlated  errors.  The  state  vector  can  be  expressed  as 


(1) 


where  the  sub  vectors  xM  and  xg  contain  master  INS  system 
error  states  and  strapdown  navigator  system  error  states 
respectively,  xJNS  and  xiMU  contain  states  that  represent  INS 
and  IMU  instrument  errors  respectively,  and  xDVS  contains 
augmenting  states  that  represent  errors  in  the  DVS.  These 
26  states  are  listed  in  Table  3.  The  error  states  modelled  in 
xM  and  xg  are  defined  as  system-indicated  values  minus  true 
values,  while  the  instrument  error  quantities  modelled  in 
xins»  ximu  anc*  xdvs  are  defined  as  factory-calibrated  values 
minus  true  values. 


3.2  Initial  Covariance  Matrix 

The  initial  covariance  matrix  P0  for  the  state  vector  is 
expressed  in  terms  of  the  initial  estimation  error  x0  as 

P0  =  E[x,,i0TI  ,  (2) 
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where  "E"  denotes  expectation  value,  the  superscript  "T”  where 
indicates  matrix  transpose,  and  the  initial  estimation  error  is 


defined  by 

PM#  =  ]  > 

(5) 

> 

(3) 

with  x0 

denoting  the  estimated  value 

of  the  true  value  of 

Pens,  “  ins,  X  ins,  1  » 

(6) 

the  initial  state  vector. 

The  initial  covariance  matrix  for  this 

TOA  system  is  expressed  in  terms  of  submatrices  as 

Ps#  =  E[xs-^  ]  , 

(7) 

PjMUj  "  1  » 

(8) 

'Pm, 

0 

0 

^M/S, 

0 

0 

p 

X  IN  So 

0 

0 

0 

Ppvs,  =  ^lXDVS,XDVS,  1  » 

(9) 

0 

0 

p 

advs„ 

0 

0 

r 

(4) 

Pm/S, 

0 

0 

Ps, 

0 

Pm/S,  =  1 

(10) 

0 

0 

0 

0 

p 

aimu,  _ 

Subvector 

State 

Description 

Coordinate  Frame 

XM 

5RMx 

8rmy 

5vmx 

8vmy 

iMx 

H’My 

'W 

master  INS  position  error  along  X  axis 
master  INS  position  error  along  Y  axis 
master  INS  velocity  error  along  X  axis 
master  INS  velocity  error  along  Y  axis 
master  INS  platform  misalignment  about  X  axis 
master  INS  platform  misalignment  about  Y  axis 
master  INS  platform  misalignment  about  Z  axis 

Wander  Azimuth1 

XINS 

amx 

AMy 

gm 

gm 

^My 

Gmz 

master  INS  x  accelerometer  bias 
master  INS  y  accelerometer  bias 
master  INS  x  gyro  bias 
master  INS  y  gyro  bias 
master  INS  z  gyro  bias 

2 

Aircraft  Body 

XDVS 

^  % 

DVS  forward  scale  factor  error 

DVS  azimuth  boresight  error 

Aircraft  Body 

*8 

8Rsx 

8rsy 

5vSx 

8VSy 

*sx 

+SZ 

strapdown  navigator  position  error  along  X  axis 
strapdown  navigator  position  error  along  Y  axis 
strapdown  navigator  velocity  error  along  X  axis 
strapdown  navigator  velocity  error  along  Y  axis 
strapdown  navigator  platform  misalignment  about  X  axis 
strapdown  navigator  platform  misalignment  about  Y  axis 
strapdown  navigator  platform  misalignment  about  Z  axis 

Wander  Azimuth 

XIMU 

O  O  O  >  > 

IMU  x  accelerometer  bias 

IMU  y  accelerometer  bias 

IMU  x  gyro  bias 

IMU  y  gyro  bias 

IMU  z  gyro  bias 

IMU  Body3 

1  Wander  azimuth  frame:  X,Y  axes  level,  Z  axis  up 

2  Aircraft  body  frame:  x  axis  forward,  y  axis  out  right  wing,  z  axis  down 

3  IMU  body  frame:  x  axis  forward,  z  axis  down,  y  axis  oriented  to  form  right-handed  coordinate  system 

Table  3.  Description  of  States  for  the  Optimal  TOA  Kalman  Filter. 
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The  submatrix  PM^  is  given  by 


J  MX 
0 
0 
0 
0 
0 
0 


J  MR 
0 
0 
0 
0 
0 


0 

0 

rLr 

0 

0 

0 

0 


0 

0 

0 

* 

7  MV 
0 
0 
0 


0 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 

7U 

0 


0 

0 

0 

0 

0 

0 


.  (11) 


where  the  variables  are  defined  as  follows: 

<y\m-  the  variance  of  the  error  in  the  initial  estimates  of 
8RMx  and  5RMy, 

< 7mt  -  the  variance  of  the  error  in  the  initial  estimates  of 

SVMX  and  8VMV> 

c  llf  -  the  variance  of  the  error  in  the  initial  estimates  of 


Kx  *My  , 


'o'm 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

^  2 

G  MV 

0 

0 

0 

0 

Pm/s0  ~ 

0 

0 

0 

^  2 

G  MV 

0 

0 

0 

■  (14) 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

In  Eq.(13),  is  defined  as  the  variance  of  the  error  in  the 


initial  estimates  of  and  . 

The  submatrices  P,NSo,  PjMU#  and  PDVSo  are  expressed  as 


1  ENSq 


<*  o  o  o  o 

o  ooo 

0  0  0  0 
0  0  0  <7  mGB  0 

.  0  0  0  0 


(15) 


&  lf}z  -  the  variance  of  the  error  in  the  initial  estimate  of 

^MZ* 

In  this  system,  position,  velocity,  wander  angle  and  heading 
in  the  IMU  strapdown  navigator  are  initialized  to  master 
INS  values  for  these  quantities.  This  implies  that  at  filter 
initialization,  the  following  relationships  between  estimation 
errors  are  true: 


i=X,Y 

sy^=_s\>  i-X,Y  (12) 

=  ■ 

Strapdown  navigator  roll  and  pitch  are  initialized 
independently  from  the  master  INS  values  because  the  IMU 
is  mounted  on  the  stabilized  antenna  while  the  master  INS  is 
mounted  on  the  airframe.  This  last  point,  together  with  the 
equations  in  Eq.(12),  results  in  the  following  expressions  for 


L  IMU0 


0 

0 

0 

0 


0 

T  SAB 
0 
0 
0 


0 

0 

'2scb 

0 


0 

0 

0 

0 

r  SOB, 


1  DVS, 


(16) 


(17) 


In  the  above  expressions,  the  variables  are  defined  as: 

^mab'  variance  of  the  master  INS  accelerometer  bias,  as 
specified  in  Table  2, 

G  mgb  ~  variance  of  the  master  INS  gyro  bias,  as  specified  in 
Table  2, 

asAB~  variance  of  the  strapdown  IMU  accelerometer  bias, 
as  specified  in  Table  1 , 


- 


'  MR 
0 
0 
0 
0 
0 
0 


0 

7  MR 
0 
0 
0 
0 
0 


0 

0 

7  MV 
0 
0 
0 
0 


0 

0 

0 

7  MV 
0 
0 
0 


0 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 

7  S* 
0 


0 

0 

0 

0 

0 

0 
.  2 

M+Z  J 


(13) 


ascB~  variance  of  the  strapdown  IMU  gyro  bias,  as 
specified  in  Table  1 , 

<y  ^  -  variance  of  the  Doppler  forward  scale  factor  error, 
as  specified  in  Section  2.1, 

aDB~  variance  of  the  Doppler  antenna  azimuth  boresight 
misalignment,  as  specified  in  Section  2. 1 . 
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3.3  State  Dynamics 

The  continuous  model  for  the  error  state  dynamics  has  the 
standard  form  applicable  to  linear  systems: 


This  yields 


*(0=  F(f)x(*)+  w(/>  .  (18) 

In  the  above  equation,  vf(t)  is  a  26  element  vector  of  zero- 
mean  white  noise  processes.  This  noise  vector  has  the  same 
structure  as  the  state  vector,  namely 


WS 

_WIMU_ 


(19) 


and  F (t)  is  a  26  by  26  element  matrix  with  the  following 
structure: 


Vz 

*r 

0 

1 

0 

0 

0 

0 

0 

L. 

Xr 

0 

1 

0 

0 

0 

Fn 

vz 

2o _ 

mZ 

0 

fr 

Fu 

Vz 

Ry 

fz 

0 

~f X 

(26) 

0 

0 

1 

0 

Rr 

mz 

-^IWy 

Q 

mn2 

1 

0 

o 

Xr 

*z 

°>IWX 

03 IW, 

0 

0 

~CO  mx 

0 

Xr 

rF 

rM 

F 

4  M/INS 

0 

0 

0 

0 

F 

*ins 

0 

0 

0 

F  = 

0 

0 

^DVS 

0 

0 

,  (20) 

0 

0 

0 

Fs 

Fr/imu 

0 

0 

0 

0 

Fjmu 

where  FM  and  Fg  are  7x7  matrices. 

Fm/ins  *s  a  7x5  matrix, 

Fs/iMU  is  a  7x5  matrix,  F|NS  is  a  5x5  matrix,  F^y  is  a  5x5 
matrix,  and  FDVS  is  a  2x2  matrix.  Using  the  expressions 
from  Eqs.(l),  (19)  and  (20)  in  Eq.(18),  the  system  dynamics 
can  be  expressed  in  terms  of  the  state  subvectors  as 


=  Fm  xm  +  Fm/ins  Xjj^s  +  WM  , 

(21) 

XINS  =  , 

(22) 

XDVS  =  Fdvs  Xdvs  +  WDVS  , 

(23) 

XS  =  Fs  *S  +  -F's/IMTJ  XIMU  +  WS  » 

(24) 

XIMU  ~  FjmxI  Xjmu  +  WIMU 

(25) 

The  elements  of  FM  correspond  to  a  true  frame  inertial  error 
model  [8]  mechanized  for  a  wander  azimuth  coordinate 
system  that  has  its  origin  at  the  master  INS,  and  that  has  X, 
Y,  Z  axes  and  wander  angle  a  as  depicted  in  Figure  2. 


Z  Up 


Figure  2.  Orientation  of  Wander  Azimuth  Frame 


V 

with  =  -^~{<oar  +  <o,Wt)  , 

Kx 

„  VT  .  .  .Vz 

Fn  =  -^-Oar,  +  ®/irr)  +  2— ’ 

•*\r 

V 

^2  =  +  <y/r,)  • 

Kr 

Rx  =  rx  +  h  , 

Rr  =  rr  +  h 

In  the  above  equation,  the  variables  are  defined  as  follows: 

Vy,  VrVz-  the  X,  Y,  and  Z  components  of  Y,  the 
master  INS  velocity  vector, 

fx*frfz~  the  X,  Y,  and  Z  components  off,  the  master 
INS  specific  force  vector, 

(D^  ,  colE  ,  a>m  -  the  X,  Y,  and  Z  components  of  oo|E, 

the  angular  velocity  vector  of  the  earth 
with  respect  to  inertial  space, 

G)Iw  ’  Wiw  >  Wiw  "  the  X,  Y,  and  Z  components  of 
the  angular  velocity  vector  of  the 
wander  azimuth  coordinate  frame 
with  respect  to  inertial  space, 

h  -  altitude  of  the  master  INS  above  the  earth 

reference  ellipsoid, 

rx,  rY  -  local  radii  of  curvature  of  the  earth  reference 
ellipsoid  along  the  X  and  Y  axes  respectively. 
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Vertical  channel  error  states  for  the  master  INS  are  not 
modelled  in  the  Kalman  filter  because  vertical  channel 
errors  are  controlled  with  a  third  order  fixed-gain  damping 
loop  implemented  internally  in  the  INS. 

The  elements  of  Fg  have  exactly  the  same  form  as  that 
expressed  in  Eq.(26)  for  FM,  except  that  the  wander  azimuth 
frame  of  interest  has  its  origin  at  the  strapdown  IMU,  and  h , 
V,  and  f  apply  to  the  strapdown  IMU  rather  than  the  master 
INS. 

The  submatrix  FM/INS  has  the  standard  expression  for  a 
strapdown  mechanization: 


0  0  0  0  0 

0  0  0  0  0 

c,,  q2  o  o  o 

c2l  Cn  0  0  0 

o  o  -c;,  -c,2  -cn 

o  o  -q,  -c„  -c,, 

0  0  -Cji  -C32  ~^33 


(27) 


where  C^.  is  the  element  in  the  rth  row  and  yth  column  of  the 
direction  cosine  matrix  which  expresses  the 

transformation  from  the  aircraft  body  frame  to  the  master 
INS  wander  azimuth  frame.  The  aircraft  body  frame  is 

defined  in  Table  3.  is  computed  as 


cos A  cos P 


-sin>4cosP  +  sinv4sini?  + 

cosAsmPsinR  cosAsmPcosR 


cr= 


-sin^lcosP 


-cosAcosR- 

sin/isinPsinP 


cos/lsinP- 

sin/lsinPcosP 


(28) 


sinP  -cosPsinP  -cosPcosP 


with 

A=a  +  H  , 

and  the  variables  defined  as: 

a  -  wander  angle  for  the  master  INS  wander  angle  frame, 

R  -  aircraft  roll, 

P  -  aircraft  pitch, 

H  -  aircraft  heading  from  North. 

The  submatrix  Fg/IMU  has  exactly  the  same  form  as  FM/1NS  in 
Eq.(27)  except  that  in  the  case  of  Fg/IMU,  the  are 

elements  of  the  direction  cosine  matrix  C|w  which  describes 
the  transformation  from  the  strapdown  IMU  body  frame  to 
the  wander  azimuth  frame  centered  at  the  IMU.  The 

strapdown  IMU  body  frame  is  described  in  Table  3.  is 
given  by  the  same  expression  as  in  Eq.(28)  with  roll,  pitch 
and  heading  interpreted  as  being  that  of  the  IMU.  In 
general,  the  roll  and  pitch  of  the  strapdown  IMU  is  close  to 


zero,  since  the  IMU  is  mounted  on  the  antenna  which  is 
physically  stabilized  in  roll  and  pitch. 

The  dynamics  of  the  error  states  in  xws,  xJMU,  and  xDVS  are 
modelled  as  first  order  Markov  processes,  resulting  in  the 
following  submatrices: 


P  MAS  0  0  0  0 

0  -Pmab  0  0  0 

0  0  -Pmob  0  0 

0  0  0  0 
0000  -/w 


(29) 


P  SAB 
0 
0 
0 
0 


0  0  0  0 

-Pmm  000 

0  -Pscb  0  0 

0  0  -  P  3GB  0 

0  0  0  - 


(30) 


F  = 

DVS 


ft  DSF 
0 


0 

-Pn* 


(31) 


In  Eqs.(29)  to  (31),  the  variables  are  defined  as: 

$mab  "  inverse  of  the  correlation  time  for  the  master  INS 
accelerometer  bias  error, 

$mgb  “  inverse  of  the  correlation  time  for  the  master  INS 
gyro  bias  error, 

-  inverse  of  the  correlation  time  for  the  strapdown 
IMU  accelerometer  bias  error, 

PSC£  -  inverse  of  the  correlation  time  for  the  strapdown 
IMU  gyro  bias  error, 

Pdsf  ~  inverse  of  the  correlation  time  for  the  DVS  forward 
scale  factor  error, 

$DB  -  inverse  of  the  correlation  time  for  the  DVS  azimuth 
boresight  error. 

The  vector  of  random  forcing  functions  in  Eq.(18),  denoted 
by  w,  is  described  in  terms  of  its  covariance  matrix  which  is 
given  in  the  continuous  formulation  by 

Pw  =  E[w(/)w(t  )t]  =  Q (t)8  (/  -  t  )  ,  (32) 

where  is  the  Dirac  delta  function  and  Q  (t)  is  a  spectral 
density  matrix.  For  this  system,  Q  is  expressed  in  terms  of 
sub  matrices  as 


Q  = 


Qm 

0 

0 


Q  In 


0 


0 

Qins 

0 

0 

0 


0 

0 

Qdvs 

0 

0 


Qm/S  0 

0  0 

0  0 

Qs  0 

0  Q)MU 


(33) 
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In  the  above  equation,  the  sub  matrix  QM  is  given  by: 


0  0  0  0 


0  0  0 


0  0  0  0 


0  0  0 


0 


Qm  = 


0 


0  qvD+  0  0 

QuAsr 

o  o  q VD  +  0 

Q  MAS F 


0 

0 


0 

0 


0  0  0 
0  0  0 
0  0  0 


0  <w  0  0 

0  0  qUGli  0 

0  0  0  qucii 


(34) 


where  q ^  and  q^^p  are  the  spectral  densities  of  white  noise 
models  to  account  for  the  effects  of  vertical  deflections  and 
INS  accelerometer  scale  factor  errors  respectively,  and  qMGN 
is  the  spectral  density  of  the  white  noise  associated  with  the 
random  drift  of  the  INS  ring  laser  gyros,  as  specified  in 
Table  2.  The  spectral  densities  qm  and  q^^p  are  given  by 
the  expressions 


Qvd  - 


2  dyD  g  2 

v 

* 


(35) 


<7 A iASF  -  a  MASr  y  (36) 

n 

where  the  variables  have  the  following  definitions: 

g  -  the  nominal  value  of  gravity, 

-  the  correlation  distance  of  the  random  deflections  of 
the  vertical, 

<y  -  the  variance  of  the  deflections  of  the  vertical, 

Vg  -  aircraft  ground  speed, 

a  -  magnitude  of  the  aircraft  acceleration  vector, 

<y  -  the  variance  of  the  master  INS  accelerometer  scale 
factor  error,  as  indicated  in  Table  2. 


A  derivation  for  the  expressions  in  Eqs.(35)  and  (36)  is 
given  in  [9]. 

The  submatrix  Qs  is  written  as: 


Q. 


9 SASF 

0  %VD  + 

Qsasf 

0  0 


0 

0 

0 

0 

Qsgmu 

0 


0 

0 

0 

0 

0 

Qsgmu 


0 
0 
0 
0 
0 
0 

saxeu  ' 
Qsgsf 


(37) 


where  qm  is  as  defined  previously,  and  qSASF,  qSGMU  and 
qSGSF  are  the  spectral  densities  of  white  noise  models  that 
account  for  the  effects  of  strapdown  IMU  accelerometer 
scale  factor  errors,  IMU  gyro  mass  unbalances  excited  by 
aircraft  accelerations,  and  the  z  gyro  scale  factor  error 
respectively.  Of  course,  gyro  mass  unbalances  are  also 
excited  by  the  constant  gravitational  field,  but  this 
component  of  the  mass  unbalance  effect  is  indistinguishable 
from  a  gyro  bias  effect  and  so  is  lumped  in  with  the 
modelled  gyro  bias  states.  It  should  also  be  noted  that  the 
effects  of  the  IMU  x  and  y  gyro  scale  factor  errors  are  not 
modelled  because  in  this  case,  these  errors  do  not  contribute 
to  inertial  platform  misalignments;  since  the  x  and  y  gyro 
input  axes  are  physically  stabilized  in  the  horizontal  plane, 
they  do  not  experience  any  significant  angular  velocities  that 
would  excite  scale  factor  error  effects.  On  the  other  hand, 
the  z  gyro  input  axis,  which  essentially  points  along  the 
local  vertical,  does  experience  a  significant  angular  velocity 
during  an  aircraft  heading  change,  so  that  during  a  turn,  the 
z  gyro  scale  factor  error  causes  a  platform  misalignment 
about  the  Z  wander  azimuth  axis. 


The  spectral  densities  qSGMU  and  qsasF  are  given  by 

the  expressions: 


4Va  2 

Qsasf  -  a  SASF  y  (38) 

n 

4V  a 

QsGMU  -  a  SGMU  *  (39) 

n 

QsGSF  -  n  y  (40) 

where  the  variables  have  the  following  definitions: 

o  -  the  variance  of  the  IMU  accelerometer  scale  factor 
error,  as  specified  in  Table  1 , 

£T  lGMU  -  the  variance  of  the  strapdown  IMU  gyro  mass 
unbalance,  as  specified  in  Table  1, 

cr  tJ3Sir  -  the  variance  of  the  strapdown  IMU  gyro  scale 
factor  error,  as  specified  in  Table  1 , 

|<oz|  -  the  magnitude  of  the  z  IMU  body  axis  component 
of  the  IMU  angular  rate  vector. 
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Derivations  for  the  expressions  in  Eqs.(38)  to  (40)  are  found 
in  [9]. 

The  submatrix  QM/S  is  defined  by  the  equation 

Qm*W*  (*-*)=  E[wm(/)ws(t)t]  (41) 

and  is  given  by 


Qm/s  “ 


0  0  0  0  0 

0  0  0  0  0 

0  0  qVD  0  0 

0  0  0  qVD  0 

0  0  0  0  0 

0  0  0  0  0 

0  0  0  0  0 


0  0  ■ 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 


(42) 


In  the  above  expression,  the  two  non-zero  terms  along  the 
diagonal  represent  the  correlation  that  exists  between 
elements  of  wM  and  ws  as  a  result  of  modelling  vertical 
deflection  effects  as  a  white  noise  process  that  drives  both 
the  master  INS  and  strapdown  navigator  velocity  error 
states. 


The  submatrices  QINS,  QIMU,  and  QDVS  are 


Qins  - 


?A 4AB 
0 
0 
0 
0 


0  0  0  0  ' 

Qmab  0  0  0 

o  qj^as  0  0 

0  0  q*m  0 

ooo  qMG3 


(43) 


Qimu 


<7*4*  o  0  0  0 

0  0  0  0 
0  0  qm  0  0 

0  0  0  <2sgb  0 

.  0  0  0  0  q„ 


(44) 


Qdvs  - 


q  DSF 

o 


0  ' 
4dB_ 


(45) 


The  spectral  densities  in  the  expressions  above  are 
computed  from  the  following  expressions: 


q^iAB  -  MAS  P  MAB  >  (46) 

q^GB  ~  mob  P  MGB  >  (47) 

qSAB  ~  SAB  P  SAB  >  (48) 

q^GB  ~  sQg  ft  ,  (49) 


q ns r  —  dsf  P  dsf  >  (50) 

9dB  ~  JOB  P  DB  >  (51) 

where  all  variables  have  been  previously  defined.  The 
spectral  density  expressions  in  Eqs.(46)  through  to  (51) 
represent  the  level  of  driving  white  noise  for  each  Markov 
process  model  that  results  in  the  steady-state  variances  for 
the  models  being  equal  to  the  initial  estimation  error 
variances  described  in  Section  3.2. 

From  the  description  of  the  state  vector  and  state  dynamics 
model  given  in  this  section,  it  is  clear  that  even  this  so- 
called  "optimar  design  is  not  truly  optimal,  in  the  sense  that 
certain  simplifying  assumptions  have  already  been  made  to 
restrict  the  number  of  modelled  states.  The  model 
simplifications  include  the  following: 

1)  Vertical  position  error,  vertical  velocity  error  and  vertical 
accelerometer  bias  states  have  not  been  modelled  in  the 
Kalman  filter  for  either  the  master  INS  or  the  IMU 
strapdown  navigator  because  these  errors  are  kept  small  by 
separate  fixed-gain  baroaltitude  damping  loops. 

2)  The  effect  of  the  scale  factor  error  in  the  master  INS  ring 
laser  gyros  has  not  been  modelled  because  it  is  very  small  (5 
ppm)  and  its  effect  is  expected  to  be  insignificant  compared 
to  the  effect  of  other  master  INS  gyro  errors. 

3)  Certain  time-correlated  errors  in  the  strapdown  IMU 
sensors,  such  as  gyro  mass  unbalances  and  gyro  scale  factor 
errors,  have  only  been  modelled,  if  at  all,  as  white  noise 
processes  driving  the  inertial  system  error  states,  because 
for  this  configuration  where  the  IMU  is  physically  stabilized 
in  roll  and  pitch,  their  effects  are  too  small  to  allow  these 
errors  to  be  observable  as  separately  modelled  states  in  the 
Kalman  filter. 

4)  Vertical  deflection  effects  have  been  modelled  as  a  white 
noise  process  driving  velocity  error  states  because  these 
effects  are  expected  to  be  too  small  to  warrant  modelling  as 
filter  states. 

Whether  or  not  a  filter  designer  feels  confident  in  making 
such  model  simplifications  right  at  the  outset  of  the  design 
process  depends  largely  on  the  designer’s  experience  and 
understanding  of  the  system  at  hand.  Typically,  it  is 
desireable  to  make  some  initial  model  simplifications  simply 
to  reduce  the  development  time  required  to  arrive  at  the 
final  Kalman  filter  configuration.  However,  if  there  is  any 
doubt  as  to  the  significance  of  certain  errors,  it  is  better  to 
err  on  the  side  of  caution  and  include  them  as  modelled 
states  in  the  initial  filter  design.  Later,  through  further 
analysis,  detailed  simulations  and  field  trials,  the  designer 
gains  additional  insights  into  the  system  error  behaviour, 
which  may  then  allow  him  to  reduce  the  model  complexity 
with  a  higher  degree  of  confidence.  The  initial  filter 
formulation  then  serves  as  the  designer’s  "optimar' 
configuration  against  which  the  performance  of  other  more 
suboptimal  designs  is  compared.  In  this  respect,  the  state 
dynamics  model  presented  in  this  section  is  considered  the 
optimal  Kalman  filter  model  for  the  TO  A  system,  despite 
containing  the  model  simplifications  described. 

3.4  Measurements 

Two  different  sets  of  measurements  are  constructed  for  use 
in  the  Kalman  filter.  One  set  is  a  velocity  matching 
measurement  between  the  DVS  and  the  master  INS.  This 
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measurement  type  is  used  to  estimate  velocity  and  attitude 
errors  in  the  master  INS.  The  other  set  is  a  velocity 
matching  measurement  Zy  between  the  master  INS  and  the 
strapdown  navigator,  which  effectively  accomplishes  the 
transfer  of  alignment. 

The  measurement  vector  Zp  is  computed  as 


Z»  =  K  S  ZJT  (52) 

=  (crTvr-v*)+(<*dS)  , 

where  the  variables  are  defined  as  follows: 

z£  -  the  master  INS/Doppler  measurement  vector 

resolved  in  the  aircraft  body  frame,  denoted  by  the 
superscript  "A", 

CT  -  the  direction  cosine  matrix  computed  from  Eq.(28) 
by  using  the  roll,  pitch,  heading  and  wander  angle 
indicated  by  the  master  INS, 

V™  -  the  velocity  vector  computed  by  the  master  INS  and 
resolved  in  the  master  wander  azimuth  frame,  as 
denoted  by  the  superscript  "MW", 

VA  -  the  aircraft  velocity  vector  indicated  by  the  DVS  and 
resolved  in  the  aircraft  body  frame, 

co  ^  -  the  angular  velocity  vector  of  the  aircraft  body 
frame  with  respect  to  an  earth-fixed  frame,  as 
computed  by  the  master  INS,  resolved  in  the  aircraft 
body  frame, 

d£  -  the  lever  arm  vector  from  the  master  INS  to  the 
DVS,  resolved  in  the  aircraft  body  frame. 

The  measurement  vector  Zy  is  calculated  by  the  expression 


z 


MW 

V 


“  [ZVX  ZVy  ZVZ  1 

=  (crvr  -  v™)  -  (cr>  *  x 


(53) 


where  the  variables  not  previously  defined  are  : 
zJT*  -  the  master  INS/strapdown  measurement  vector, 

resolved  in  the  master  INS  wander  azimuth  frame, 

Vssw  -  velocity  vector  computed  by  the  IMU  strapdown 
navigator,  and  resolved  in  the  strapdown  IMU 
wander  azimuth  frame,  denoted  by  the 
superscript  "SW", 

d£  -  the  lever  arm  vector  from  the  master  INS  to  the 

strapdown  IMU,  resolved  in  the  aircraft  body  frame, 

-  the  direction  cosine  matrix  describing  the 
transformation  from  the  strapdown  IMU  wander 
azimuth  frame  to  the  master  INS  wander  azimuth 
frame. 


The  matrix  is  computed  by 


pMW  _  pE 

'“'SW  —  ''MwSw 


(54) 


where  C*w  and  are  the  direction  cosine  matrices 

describing  respectively  the  transformation  from  the 
strapdown  IMU  wander  azimuth  frame  to  a  specified  earth- 
fixed  frame,  and  the  transformation  from  the  master  INS 
wander  azimuth  frame  to  the  earth-fixed  frame.  The  matrix 

CL  is  calculated  as 


cosa  cosL 


-sina  cosL  sinL 


CE  - 

'"SW  ” 


sina  cos/  + 
cos  a  sin  L  sin/ 


cosa  cos /  - 
sina  sinLsin/ 


-cosL  sin/ 


,  (55) 


sina  sin  /  -  cosa  sin  /  +  T  . 

cosa  sin L cos/  sina  sin L cos/  u  1 


where  a,  L  and  /  are  the  wander  angle,  latitude  and 
longitude  computed  by  the  IMU  strapdown  navigator.  The 

matrix  CJL  is  calculated  with  exactly  the  same  expression 
as  in  Eq.(55)  except  that  the  wander  angle,  latitude  and 
longitude  used  are  those  indicated  by  the  master  INS. 


The  Kalman  filter  actually  processes  only  zVx  and  zVy,  the 
horizontal  components  of  z ,  and  zD  and  zD  ,  the  x  and  y 

components  of  zA.  Also,  the  master  INS/Doppler 
measurements  are  processed  only  during  nominally  straight 
and  level  flight  to  avoid  additional  inaccuracies  in  the  DVS 
that  result  when  the  aircraft  is  steeply  rolled  or  pitched.  The 
measurement  vector  z  that  is  processed  by  the  filter  is  then 
written  as 


z  = 


z. 

L  yr  J 


(56) 


3.5  Measurement  Models 

The  linear  model  for  discrete  measurements  is  given  by 

z=  Hx+  v  ,  (57) 

where  H  is  the  measurement  matrix  with  dimension  4x26 
and  v  is  a  4  element  vector  of  measurement  errors  that  are 
assumed  to  be  zero-mean  sequences  uncorrelated  in  time. 
The  measurement  matrix  is  derived  by  perturbing  the  right 
hand  side  of  Eqs.(52)  and  (53)  and  expressing  the  result  in 
terms  of  the  modelled  error  states.  The  following 
assumptions  are  made  in  this  derivation:  1)  the  effect  of 
errors  in  computing  the  coxd  terms  and  the  transformation 

matrix  can  be  neglected  (these  small  effects  are 

included  in  the  measurement  noise  model),  2)  the  x  and  y 
axes  of  the  aircraft  body  frame  are  nominally  in  the 
horizontal  plane  when  master  INS/Doppler  measurements 
are  constructed,  which  allows  the  measurement  error  terms 
containing  vertical  velocity  errors  to  be  neglected,  and  3) 
vertical  velocities  are  small  enough  (i.e.  <  10  m/s)  to  allow 
measurement  error  terms  containing  vertical  velocity  to  be 
neglected.  These  last  two  assumptions  are  valid  since,  as 
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pointed  out  earlier,  master  INS/Doppler  measurements  are 
processed  only  during  nominally  straight  and  level  flight. 
The  resulting  measurement  matrix  is  expressed  in  terms  of 
sub  matrices  as: 


H  = 


Hd  0  H, 

UM  UDV* 

Hv  0  0 

VM 


0 

HV. 


0 

0 


(58) 


The  submatrices  and  ,  which  have  dimensions  of 

2x7  and  2x2  respectively,  are  given  by 


0  0  Qt  C21  CMVMr  -CMV„x 
0  0  Cu  Cn  CJHi  -C»V„x 


(CuvUx-cnvUr) 

(CaV„x-CaVUr) 


(59) 


HdDv« 


(60) 


where  the  variables  are  defined  as: 


V 

the  elements  of  , 

the  x  component  of  V* 

VMX’  VMy 

the  X  and  Y  components  of  V^ , 

v  v 

the  x  and  y  components  of  V*  which  is 

computed  as 

The  submatrices  Hv 
given  below: 


\jA  _  rMWTyMW 
VM  "  VM 


(61) 


and  Hv  ,  both  of  dimension  2x7,  are 


p  0  -1  0  0  0  0 

n^Lo  o  o  -looo 

ro  o  q,  c;2  o  o  o’ 
^  [o  o  q,  q,  o  o  o_ 


(62) 


(63) 


where  the  C'  are  the  elements  of  . 

The  measurement  noise  vector  v  is  characterized  by  its 
covariance  matrix  R  which  is  given  by 


R  =  E[vvT]  = 


0 

0 

0 


0 

0 


0 

0 


0 

0 

0 

av  J 


(64) 


In  the  above  equation,  the  variables  are  defined  as 
<r  ^  -  the  variance  of  the  measurement  noise  for  zD  , 


<j  *  -  the  variance  of  the  measurement  noise  for  zn  » 

y  uy 

cr  l  -  the  variance  of  the  measurement  noise  for  zv  and 
Y  vx 

V 

The  main  contributor  to  the  measurement  noise  for  z and 

is  the  noise  on  the  DVS  velocity  measurements,  which  is 
specified  in  Section  2.1.  The  measurement  noise  modelled 
for  zyx  and  z^  includes  the  effects  of  quantization  of  master 

INS  velocity  data,  and  the  effects  of  airframe  flexing  which 
causes  errors  in  computing  relative  velocity  between  the 
master  INS  and  strapdown  IMU. 

The  fact  that  the  matrix  R  is  diagonal  implies  that  the 
elements  of  v  are  uncorrelated  with  each  other.  Strictly 
speaking,  this  is  not  true.  There  is  a  correlation  between  the 

measurement  noise  for  z ^  and  zD?  due  to  the  fact  that  the 

noise  associated  with  the  DVS  forward  and  lateral  velocity 
measurements  is  somewhat  correlated.  However,  by 
assuming  that  the  noise  elements  are  uncorrelated,  there  is 
only  a  small  loss  in  optimality,  while  there  is  a  significant 
advantage  gained  in  that  the  scalar  components  of  the 
measurement  vector  can  be  processed  sequentially  by  the 
filter  at  a  given  update  time,  rather  than  processing  the  z 
vector  as  a  whole.  This  allows  matrix  inversion  to  be 
avoided,  thereby  increasing  computational  efficiency. 

3.6  Simulation  Results 

A  high-fidelity  computer  simulation  is  an  extremely  useful 
tool  in  evaluating  a  Kalman  filter  design  and  predicting  filter 
performance.  Such  a  simulation  was  performed  for  the 
optimal  TO  A  filter  described  in  this  section. 

The  simulation  package  that  was  used  generates  simulated 
master  INS,  strapdown  IMU,  DVS  and  air  data,  which  are 
then  used  as  inputs  to  a  processing  package.  The  simulated 
sensor  data  corresponds  to  a  particular  user-defined  flight 
profile,  and  includes  the  effects  of  all  the  sensor  errors 
discussed  in  Section  2.1,  as  well  as  other  errors  like 
quantization  of  the  sensor  output  data.  The  simulation 
package  also  provides  files  containing  true  position, 
velocity,  attitude  and  heading  as  a  function  of  time  for  both 
the  master  INS  and  strapdown  IMU.  These  files  serve  as 
the  reference  against  which  the  processing  package  outputs 
are  compared  in  order  to  evaluate  Kalman  filter  estimation 
errors.  The  processing  package  consists  of  the  Kalman 
filter,  strapdown  navigator  and  baroaltitude  algorithm  shown 
in  the  block  diagram  of  Figure  1 . 

For  the  Kalman  filter  processor,  the  Kalman  filter  equations 
are  implemented  in  their  discrete  form  using  Bierman’s  U-D 
factorization  algorithm  [10]  to  efficiently  propagate  and 
update  the  error  covariance  matrix  .  Other  features  that  are 
used  to  enhance  computational  efficiency  include  sequential 
processing  of  measurements,  sparse  matrix  multiplication 
techniques  and  exploitation  of  the  block  structure  of  certain 
matrices  to  increase  the  execution  speed  of  matrix 
operations.  In  the  discrete  Kalman  filter  formulation,  the 
model  for  the  error  state  dynamics  as  expressed  in  Eq.(18) 
is  converted  to  a  discrete-time  model  of  the  form: 
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*k+.  =  *k*k  +  wk  .  (65) 

where  wk  is  a  vector  of  zero-mean  white  noise  sequences 
with  covariance  matrix  Qk,  and  xk  and  xk+1  refer  to  the 
values  of  the  state  vector  at  times  tk  and  tk+1  respectively. 

The  interval  A Tk  s  (ft+l  -  tk)  is  the  interval  between  Kalman 
filter  measurement  updates.  The  update  interval  for  this 
simulation  is  10  seconds.  To  obtain  the  transition  matrix 

<I>k ,  a  transition  matrix  is  first  computed  over  a  smaller 

sub  interval,  referred  to  as  the  propagation  interval,  by 
retaining  the  first  three  terms  of  the  matrix  exponential 
expression  below: 


=  expfFjAf j) 

=  I+  F,Af,  +  |f,2(a +  If/ca^)^...  ,(66) 

where  A  tj  is  the  propagation  interval  defined  by 
A tj  s  (/  -  t.) ,  and  ¥s  is  the  average  value  of  F (t)  over 

A tjy  with  F (t)  given  by  the  expression  in  Eq.(20).  In  the 
simulation,  the  basic  propagation  interval  is  2  seconds.  The 
transition  matrix  is  recalculated  every  propagation 
interval,  and  <&k  is  then  calculated  as 


~  A  A  A  A  A 
k  J+4J+3J+2J+1J 


(67) 


The  covariance  matrix  Qk  is  computed  from  the  expression 


Qk  =  e^Q(r  )(er™'  Y  dr  (68) 

by  expanding  the  matrix  exponential  terms,  multiplying  out 
and  integrating  the  products  with  the  assumption  that  Q (t), 
the  spectral  density  matrix  given  in  Eq.(33),  is  constant  over 

the  interval  A 7^  .  In  the  above  expression,  Fk+1  is  the  value 
ofFft)  atft+/. 


For  this  simulation,  the  elements  of  the  submatrices  PM>  and 

PSj  in  the  initial  error  covariance  matrix  PQ  were  assigned 

values  consistent  with  expected  system  errors  in  the  INS 
following  a  stationary  ground  alignment.  The  flight  profile 
used  for  the  simulation  is  depicted  in  Figure  3.  A 
description  of  the  various  manoeuvres  included  in  the 
simulated  flight  profile  is  given  in  Table  4.  It  should  be 
noted  that  the  DVS  data  is  not  used  to  construct  Kalman 
filter  measurements  until  cruising  altitude  is  reached.  The 
velocity  matching  measurements  between  the  strapdown 
navigator  and  master  INS  are  constructed  and  processed 
right  from  time  zero. 

The  performance  of  the  optimal  TO  A  Kalman  filter  is 
indicated  in  Figures  4  to  8.  The  solid  traces  in  the  plots 
indicate  the  strapdown  navigator  velocity,  attitude  and 
heading  estimation  errors  over  the  course  of  the  flight, 
assuming  closed  loop  correction  of  strapdown  navigator 
quantities  with  the  filter-estimated  error  states.  The 
estimation  errors  are  obtained  by  subtracting  the  '’true” 
velocity,  attitude  and  heading  values,  as  generated  by  the 
simulation  package,  from  the  corresponding  values 
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computed  by  the  strapdown  navigator  in  the  processing 
package.  The  dashed  lines  show  the  filter-predicted 
standard  deviation  bounds  for  these  estimation  errors.  The 
sharp  decreases  in  the  standard  deviation  bounds  generally 
occur  right  after  turns,  when  DVS  error  states  and  inertial 
platform  misalignments  about  the  Z  axis  become  separately 
observable  from  the  other  error  states.  It  can  be  seen  that 
the  time  histories  of  the  estimation  errors  are  fairly 
consistent  with  the  predicted  standard  deviation  bounds. 


TIME 

00 

DESCRIPTION  OF  MANOEUVRE 

0 

Stationary;  heading  is  150  degrees 

3 

Start  accelerating  down  runway 

75 

Takeoff;  aircraft  pitch  increases  to  6  degrees 

116 

Achieve  and  maintain  cruising  speed  of  108  m/s 

173 

Reach  and  maintain  cruising  altitude  of  1000  m; 
aircraft  pitch  returns  to  zero 

330 

Enter  racetrack;  aircraft  roll  increases  to  30  degrees 

392 

Complete  first  180  degree  turn  of  racetrack 

500 

Enter  second  turn  of  racetrack 

562 

Complete  second  180  degree  turn  of  racetrack; 
aircraft  heading  returns  to  150  degrees 

870 

Start  turn  to  heading  of  90  degrees 

891 

Complete  turn  to  heading  of  90  degrees 

941 

Start  S-manoeuvre 

1067 

Complete  S-manoeuvre;  aircraft  heading  returns  to  90 
degrees 

3400 

Start  S-manoeuvre 

3506 

Complete  S-manoeuvre;  heading  held  at  150  degrees 

4500 

Complete  profile 

Table  4.  Description  of  Manoeuvres  in  Simulated  Flight 
Profile. 
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Figure  4.  Strapdown  Navigator  North  Velocity  Error  and 
Predicted  Error  Standard  Deviation  (Optimal 
Filter). 


Figure  5.  Strapdown  Navigator  East  Velocity  Error  and 
Predicted  Error  Standard  Deviation  (Optimal 
Filter). 


Figure  6.  Strapdown  Navigator  Roll  Error  and  Predicted 


Figure  7.  Strapdown  Navigator  Pitch  Error  and  Predicted 


Error  Standard  Deviation  (Optimal  Filter). 


Error  Standard  Deviation  (Optimal  Filter). 


Figure  8.  Strapdown  Navigator  Heading  Error  and 
Predicted  Error  Standard  Deviation 
(Optimal  Filter). 
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4.  SUBOPTIMAL  TOA  KALMAN  FILTER  DESIGN 
4.1  State  Decoupling 

If  certain  parts  of  a  system  state  vector  are  only  weakly 
coupled,  it  is  often  possible  to  split  the  one  Kalman  filter 
into  two  or  more  lower  order  filters.  This  can  reduce  the 
computational  burden  by  a  significant  amount  since  the 
number  of  multiplications  required  to  perform  the  most 
intensive  filter  task  -  the  solution  of  the  error  covariance 
equations  -  varies  roughly  as  the  third  power  of  the  number 
of  states. 

In  this  section,  it  is  shown  that  the  optimal  TOA  Kalman 
filter  described  in  Section  3  can  effectively  be  split  into  two 
filters.  The  Doppler  damping  of  the  master  INS  is 
performed  in  one  filter,  while  the  transfer  of  alignment  from 
the  master  INS  to  the  strapdown  navigator  is  performed  in  a 
second  filter  which  is  mathematically  decoupled  from  the 
first. 

The  means  by  which  the  original  Kalman  filter  can  be 
decoupled  becomes  more  apparent  if  one  considers  an 
alternate  filter  formulation  for  which  the  differences  between 
the  absolute  errors  in  the  master  INS  and  strapdown 
navigator  are  modelled,  instead  of  the  absolute  strapdown 
navigator  errors  themselves.  Such  a  filter  formulation  is 
based  on  the  fundamental  insight  that  measurements 
constructed  by  comparing  equivalent  information  from  two 
systems  with  the  same  error  dynamics  will  only  allow 
differences  between  the  two  systems  to  be  observable, 
rather  than  the  absolute  errors  in  each  system.  Thus  the 
velocity  matching  measurement  between  the  master  INS  and 
strapdown  navigator  provides  direct  observability  only  for 

x'  where 


and  it  has  been  assumed  that  FM  *  Fs.  This  assumption  is 
valid  as  long  as  the  axes  of  the  master  INS  and  strapdown 
navigator  wander  azimuth  frames  are  nearly  coincident  (i.e. 

*  I,  the  identity  matrix)  and  the  relative  motion 
between  the  master  INS  and  strapdown  IMU  is  small 
compared  to  the  nominal  values  of  velocity  and  specific 
force  appearing  in  the  F  matrices.  Both  these  conditions  are 
true  in  this  case. 

The  measurement  model  for  the  master  INS/strapdown 

velocity  matching  measurements  is  obtained  in  terms  of 

by  first  writing  out  the  following  expression,  using  Eqs.(56), 
(57)  and  (58): 


where  HVm  and  HVg  are  given  by  Eqs.(62)  and  (63). 
Substituting  xs  =  x'  +  xM  into  the  equation  above  and 

assuming  again  that  ~  I  yields 

.  (73) 

The  form  of  the  above  equation  clearly  confirms  that  only 
x'  is  fundamentally  observable  from  the  Zy  measurement. 

In  order  for  the  optimal  TOA  filter  to  be  decoupled,  the 
estimation  of  one  set  of  states  must  be  mathematically 
decoupled  from  the  estimation  of  the  second  set  x2,  where  in 
this  case,  it  is  desired  that 


x:  =  x*  -  x. 


5R4 

sr; 

sY 

<5V' 

*x 

*Y4 

*4 

*4 

l  *4  j 


(69) 


and  not  for  xg  or  xM  themselves,  whereas  the  DVS 
measurements,  as  defined  in  Eq.(52)  do  allow  elements  of 
xM  to  be  observable.  From  the  viewpoint  of  observability 
then,  an  alternative  selection  of  inertial  system  error  states 
for  the  original  fully  coupled  filter  could  just  as  well  be  xM 

and  x'  instead  of  xM  and  xg.  In  that  case,  the  dynamics  of 


For  Xj  to  be  decoupled  from  x 2,  there  must  be  no  modelled 
correlation  between  xt  and  x^.  In  other  words,  the  cross¬ 
covariance  matrix  of  Xj  and  x2  must  be  zero  at  all  times. 
There  are  generally  three  ways  that  correlation  between  xt 
and  x 2  can  be  introduced  into  the  Kalman  filter: 

1)  through  the  measurement  model, 

2)  through  the  model  of  the  system  dynamics, 

3)  through  the  initialization  of  the  covariance  matrix. 

The  measurement  model  for  zy *  and  zy* ,  as  expressed  in 
Eq.(73),  is  only  a  function  of  x^  and  the  measurement 


the  state  vector  x'  are  derived  by  subtracting  Eq.(21)  from 
Eq.(24)  to  give 

XS  =  +  (^S/IMUXIMU  “  ^M/INSXINs)  +  WS  »  (70) 

where 

=  ws  “  wm  *  (71) 


model  for  z ^  and  zD ^  discussed  in  Section  3.5  is  only  a 

function  of  xr  So  these  measurement  models  are  already  in 
a  form  that  avoids  correlation  between  xt  and  x2. 

Examination  of  the  error  dynamics  models  in  Eq.(70)  and 
(21)  indicates  that  correlation  will  develop  between  xt  and  x 2 

because  both  xM  and  x'  are  driven  by  a  common  error  state 

term  F^x^ .  However,  the  error  covariance  for  the  states 
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in  xJMU  is  modelled  as  being  significantly  larger  than  the 
error  covariance  for  xJNS,  so  that  the  effect  of  F^x^  on  the 

estimation  of  x'  is  expected  to  be  much  less  than  the  effect 
of  the  Fjj^x^  term,  and  can  therefore  be  neglected.  There 
is  also  a  certain  amount  of  correlation  between  wM  and  w '  . 

The  cross  spectral  density  matrix  for  these  two  process 
noise  vectors,  using  Eqs.(34)  and  (42),  is  found  to  be 


However,  consider  the  spectral  density  matrix  for  w'  .  This 


matrix  Q'  is  expressed  in  terms  of  previously  defined 
spectral  density  matrices  as 


=Qm  +  Qs-2Qm* 


which  yields 


(76) 


0  0  0 

0  0  0 

o  0  3W+ 

^UASF 


q:  = 


0  0 
0  0 


0 

0 


0  0  0  0 


0  0  0  0 


0  0 

0  0 

0  0 

0  0 

Qsgwj*  0 

Qmgn 

0  <isGUU+ 

Qugn 

0  0 


0 

^SGSF+  Qmgn 


The  principal  terms  in  the  above  expression  that  affect  the 
estimation  of  i'  are  qSGMU  and  qSGSF,  which,  in  general,  are 
significantly  larger  than  the  qMGN  terms  that  appear  in  the 

same  locations  in  the  matrix  .  Consequently,  it  can  be 


assumed  that  =  0  with  little  effect  on  the  estimation  of 

x' .  It  may  be  noticed  that  q ^  does  not  appear  in  Eq.(77). 

This  is  consistent  with  the  expectation  that  vertical  deflection 
errors,  which  affect  both  xs  and  xM  in  the  same  way,  should 
not  affect  the  difference  states.  Based  on  the  above 
discussion,  the  state  dynamics  models  for  the  two  decoupled 
filters  can  be  written  as 


where 


i,  =  F.x,  +  w, 
i2  =  F2x2  +  w2 


(78) 
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(79) 
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*S/IMU 

" w; ' 

= 

,  w2  = 

0 
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IMU  _ 

_WIMU_ 

(80) 


The  initial  cross-covariance  matrix  between  xt  and  x2  is 
expressed  as 


Pl2<  =  E[f4x^  ]  = 


e[5m.5C  ]  Elx^x^  ] 

1  EI*INS,*IMU,  1 

1  EI*DVS,*IMU,  1 


(81) 


All  of  the  terms  in  the  above  expression  are  found  to  be 
zero  except  for  the  initial  cross-covariance  matrix  between 

xM  and  x'.  This  term  is  evaluated,  with  the  help  of 
Eqs.(ll)  and  (14),  as 


E[XM#XS,  ]  -  ^M/S, 

"0  0  0 

0  0  0 

0  0  0 

=  000 
0  0  0 

0  0  0 

0  0  0 


0  0  0  0" 

0  0  0  0 

0  0  0  0 

0  0  0  0 

0  0  0 
0  0  0 
0  0  0  0_ 


(82) 


The  magnitude  of  the  non-zero  terms  in  the  above 
expression  is  much  smaller  than  the  initial  variance  <7^ 

assigned  to  the  estimation  error  for  <f> and  <j>^ .  This 

variance  is  large  to  reflect  the  large  uncertainty  in  the 
knowledge  of  the  initial  roll  and  pitch  of  the  IMU.  Under 
these  conditions  then,  it  can  be  assumed  that  P^^,  with 
little  consequence  for  the  estimation  of  x2. 

In  summary  then,  it  can  safely  be  assumed  that  the  three 
conditions  to  allow  state  decoupling,  as  stated  previously  in 
this  section,  are  satisfied  for  the  two  vectors  xt  and  x 2,  and 
consequently,  the  optimal  TOA  filter  can  be  mechanized  as 
two  decoupled  filters. 
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It  should  be  noted  that  for  the  purposes  of  closed  loop 
correction  of  the  strapdown  navigator  parameters,  it  is  still 

an  estimated  value  of  xs  that  is  required,  not  x' . 

Consequently,  for  this  decoupled  filter  configuration  where 
xs  is  not  explicitly  estimated,  it  must  be  constructed  when  it 
is  needed  as 

+  >  (83) 
where  the  symbol A  denotes  estimated  quantities. 

4.2  State  Deletion 

If  certain  error  states  that  are  being  modelled  in  the  filter  are 
too  small  to  be  observable  with  the  given  measurement 
models,  then  they  can  often  be  eliminated  with  little  impact 
on  the  filter  performance.  For  example,  consider  the  plots 
in  Figures  9  to  1 1  obtained  from  the  simulation  run  with  the 
optimal  filter.  The  solid  traces  in  these  plots  show  the  time 
histories  of  the  master  INS  gyro  bias  state  estimation  errors, 
and  the  dashed  lines  show  the  filter-computed  standard 
deviations  for  these  estimation  errors.  It  is  obvious  that  the 
predicted  standard  deviations  do  not  change  significantly 
from  their  initial  values  over  the  course  of  the  run.  This  is  a 
clear  indication  that  these  states  are  too  small  to  be 
observable  by  the  filter. 

When  states  are  eliminated,  it  is  usual  to  attempt  to 
compensate  for  their  loss  by  adding  a  component  to  the 
process  noise.  For  example,  from  the  expression  for  FM/INS 
in  Eq.(27),  the  first-order  effect  of  a  gyro  bias  state  is  to 
cause  a  linear  growth  in  inertial  platform  misalignments. 
Thus,  the  variance  growth  for  the  platform  misalignment 
estimation  error  as  a  result  of  a  modelled  gyro  bias  state  is 
given  approximately  by 

"UV-vlcm*'  •  (84) 

If  the  effects  of  a  gyro  bias  were  modelled  by  a  white  noise 
process  with  spectral  density  qMGBy  then  the  variance  growth 
for  the  platform  misalignment  estimation  errors  is  expressed 
as 

1  •  (85) 

One  criterion  that  the  filter  designer  may  use  to  assign  an 
appropriate  value  to  qMGB  is  to  select  a  value  of  qMGB  that 

results  in  the  a  ^  buildup  being  equal  for  both  cases  at  a 
chosen  time  T.  This  condition  is  expressed  as 

°lc,T'  =  quGtT  ,  (86) 

which  yields 

•  (87) 

The  choice  of  T  is  itself  fairly  arbitrary,  but  a  suitable 
choice  is  the  filter  settling  time,  which  is  in  the  order  of 
about  1000  seconds.  The  quantity  qMGB  computed  from  the 
above  expression  is  then  added  to  the  qMGN  terms  in  the 
spectral  density  matrix  QM  to  compensate  for  the  deletion  of 
the  gyro  bias  states. 

Another  case  for  deleting  states  can  be  made  when  it  is  not 
important  to  know  the  state  estimate,  and  the  state  does  not 


Figure  9.  Master  INS  X  Gyro  Bias  Estimation  Error  and 
Predicted  Error  Standard  Deviation  (Optimal 
Filter). 


Figure  10.  Master  INS  Y  Gyro  Bias  Estimation  Error  and 
Predicted  Error  Standard  Deviation  (Optimal 
Filter). 


Figure  11.  Master  INS  X  Gyro  Bias  Estimation  Error  and 
Predicted  Error  Standard  Deviation  (Optimal 
Filter). 
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have  a  significant  effect  on  the  estimation  of  other  states  of 
interest.  A  good  example  of  this  is  the  position  error  states 
modelled  for  the  master  INS  and  the  strapdown  navigator. 
The  primary  requirement  for  the  TOA  filter  is  to  control 
attitude  errors  and  velocity  errors  in  the  strapdown 
navigator.  Absolute  positional  accuracy  is  not  a 
requirement.  Furthermore,  deletion  of  the  position  error 
states  does  not  impact  on  estimation  of  the  other  states. 
This  can  be  seen  by  considering  the  measurement  model  and 
error  dynamics  model  for  the  TOA  filter.  Since  the 
measurement  model  does  not  include  any  terms  that  contain 
the  position  error  states,  then  obviously  it  is  not  affected  by 
the  absence  of  these  states.  With  regard  to  the  error 
dynamics,  from  examination  of  the  expression  for  FM  in 
Eq.(26),  it  can  be  seen  that  position  error  terms  which  drive 
the  velocity  errors  and  platform  misalignments  are  weak 
compared  to  other  driving  terms  for  these  states.  The 
dominant  term  that  would  be  omitted  as  a  result  of  deleting 


the  position  error  states  is  of  the  form 


^-SR 

R 


To  first 


order,  this  term  causes  a  linear  growth  of  platform  tilts. 
Following  a  similar  approach  to  that  taken  for  the  gyro  bias 
states,  an  appropriate  selection  for  the  spectral  density  of  an 
equivalent  white  noise  process  is 


where  is  chosen  to  be  the  expected  variance  of  the 
position  error  over  the  entire  flight,  and  T  ,  as  before,  is 
selected  to  be  the  filter  settling  time.  This  new  spectral 
density  term  is  included  in  QM  by  summing  it  with  the  qMGN 
terms  that  affect  the  estimation  of  platform  tilts.  A  similar 

spectral  density  term  q ^  can  be  incorporated  into  Q'  to 

compensate  for  the  deletion  of  position  states  from  the  x' 
vector. 

4.3  Simulation  Results 

An  identical  simulation  run  to  that  performed  for  the  optimal 
TOA  filter  was  conducted  with  the  suboptimal  filter  design. 
For  this  run,  the  modifications  to  the  measurement  and  error 
dynamics  model  discussed  in  this  section  were  implemented 
to  realize  two  decoupled  filters  without  position  error  states 
or  master  INS  gyro  bias  states. 

The  results  of  the  run  are  shown  in  Figures  12  to  16  which 
depict  the  velocity,  attitude  and  heading  estimation  errors  of 
the  strapdown  navigator,  along  with  associated  filter- 
predicted  standard  deviations,  after  closed  loop  correction 
with  the  filter-estimated  error  quantities.  From  comparison 
to  the  corresponding  plots  in  Figures  4  to  8,  it  is  clear  that 
the  suboptimal  filter  performance  is  very  nearly  the  same  as 
that  of  the  optimal  configuration.  These  results  confirm  the 
validity  of  the  model  simplifications  discussed  in  this 
section. 

5.  CONCLUSIONS 

The  process  of  designing  a  suboptimal  /ransfer-tff-nlignment 
(TOA)  Kalman  filter  for  a  SAR  motion  compensation 
application  has  been  detailed  at  some  length.  While  a 


certain  amount  of  the  information  presented  is  fairly  unique 
to  this  particular  application,  the  criteria  used  in  making  the 
filter  model  simplifications  provide  useful  guidelines  for 
general  suboptimal  filter  design.  Also,  much  of  the 
discussion  regarding  inertial  system  behaviour  is  of  course 
relevant  to  aerospace  navigation  applications  involving 
inertial  technology.  Further  information  on  general 
principles  of  suboptimal  filter  design  and  implementation 
can  be  found  in  [6] . 

Specifically,  two  principal  techniques  of  Kalman  filter  model 
simplification,  namely  state  deletion  and  state  decoupling, 
have  been  demonstrated  for  the  TOA  filter  example. 
Starting  with  a  description  of  the  optimal  Kalman  filter,  the 
process  of  designing  modifications  to  accommodate  state 
deletion  and  decoupling  has  been  discussed,  along  with  the 
rationale  for  these  modifications.  Simulation  results  for  the 
optimal  and  suboptimal  filter  configurations  were  presented 
to  confirm  that  the  suboptimal  filter  design  provided  similar 
performance  to  the  optimal  filter. 
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Figure  12.  Strapdown  Navigator  North  Velocity  Error  and 
Predicted  Error  Standard  Deviation  (Suboptimal 
falter). 


Figure  13.  Strapdown  Navigator  East  Velocity  Error  and 
Predicted  Error  Standard  Deviation  (Suboptimal 
Filter). 


Figure  14.  Strapdown  Navigator  Roll  Error  and  Predicted 
Error  Standard  Deviation  (Suboptimal  Filter). 


Figure  15.  Strapdown  Navigator  Pitch  Error  and  Predicted 
Error  Standard  Deviation  (Suboptimal  Filter). 


Figure  16.  Strapdown  Navigator  Heading  Error 
and  Predicted  Error  Standard  Deviation 
(Suboptimal  Filter). 
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